#include "predict.h"
#include "visual.h"
#include "yaml_config.h"
#include "opencv.hpp"
#include <librealsense2/rs.hpp>
#include "task.h"

void Task_walk(RobotController &robot_a1, Custom &custom)
{
  /* 初始化walk的相关参数 */
  custom.cmd.mode = 2;
  custom.cmd.gaitType = 2;
  custom.cmd.velocity[0] = 0;
  custom.cmd.velocity[1] = 0;
  custom.cmd.yawSpeed = 0;
  custom.cmd.footRaiseHeight = 0.1;
  custom.udp.SetSend(custom.cmd);
  
  while (1)
  {
    custom.motiontime += custom.dt_ms; // ms
    custom._second += custom.dt; //s

    custom.udp.GetRecv(custom.state);
    mtx.lock();
    custom.cmd.velocity[0] = robot_a1.forward_speed;
    custom.cmd.velocity[1] = robot_a1.side_speed;
    custom.cmd.yawSpeed = robot_a1.yaw_speed;
    mtx.unlock();

    custom.udp.SetSend(custom.cmd);
    
    custom.log();
    if (motiontime % 700 == 0)
    {
      string dog_mode;
      switch (robot_a1.mode)
      {
        case 0: dog_mode = string("Forward"); break;
        case 1: dog_mode = string("Obstacle"); break;
        case 2: dog_mode = string("Horizontal"); break;
        case 3: dog_mode = string("LoseLine"); break;
        default:
          break;
      }
      cout << "*****************************" << endl;
      cout << "forward = " << robot_a1.forward_speed << endl;
      cout << "side    = " << robot_a1.side_speed << endl;
      cout << "yaw     = " << robot_a1.yaw_speed << endl;
      cout << "mode    = " << dog_mode << endl;
      cout << "*****************************" << endl
          << endl;
    }

    this_thread::sleep_for(chrono::milliseconds(custom.dt_ms));
  }
}
